
# Read data (= reference trajectory), optimize PID (using nomad)



import numpy as np
import matplotlib.pyplot as plt
from math import pi, sin, cos, sqrt
from random import gauss, uniform, seed
import sys
import csv


Gkp = 0
Gki = 0
Gkd = 0

# true = nomad is active
# false = receive kp,ki,kd from command line and generate control history
Gflagopt = True

Glistctrl = []

GDados = []

#=============================================================================
Gintegral = 0
Ge0 = 0

def pid (e1 \
       ,kp = 0.00001 \
       ,ki = 0.0001 \
       ,kd = 0.005) :

   global Gintegral
   global Ge0

   ctrl = (e1         * kp) +  \
          (Gintegral  * ki) +  \
          ((e1 - Ge0) * kd)

   Gintegral = Gintegral + 0.5 * (Ge0 + e1)

   Ge0 = e1

   return ctrl




#=============================================================================

# For reproducibility purposes
rseed = 123
seed(rseed)


GReagenteTotal     = 0.0
GReagenteTotalAlvo = 1000.0
GConcentraEntrada  = 1.0      # gramas por litro
GVolumeEntrada     = 10.0     # litros
GVolumeSaida       = 10.0     # litros
GVolumeReator      = 1000.0   # litros
GTempo             = 0



#-----------------------------------------------------------
def torneira():

   global GReagenteTotal
   global GReagenteTotalAlvo
   global Gflagopt
   global Glistctrl
   global GDados
   global GTempo

   a = 1 + 0.01 * pid (GDados[GTempo] - GReagenteTotal, Gkp,Gki,Gkd)

   # Se nomad não está funcionando, guarde controle para arquivar ao final.
   # Se nomad funcionando, isto apenas perderia tempo.
   if not Gflagopt:
      Glistctrl.append (a)

   r = a 
   if r < 0: r = 0
   if r > 2: r = 2
   return r

#-----------------------------------------------------------
def AtualizarReator():

   global GReagenteTotal
   global GConcentraEntrada
   global GVolumeEntrada
   global GTempo


   GReagenteTotal = GReagenteTotal + \
                    torneira()    * (GVolumeEntrada * GConcentraEntrada) - \
                    (GVolumeSaida * (GReagenteTotal / GVolumeReator)) 

#-----------------------------------------------------------



try:
    arq = open(sys.argv[1], "r")
    valctrl = arq.read().split()

    Gkp = float(valctrl[0])
    Gki = float(valctrl[1])
    Gkd = float(valctrl[2])


except:
    Gflagopt = False

    print ("Não otimizando: apenas geração de controle.")

if not Gflagopt:
    try:
        f = open("otimo.csv", "r")
        valctrl = f.read().split()

        Gkp = float(valctrl[2])
        Gki = float(valctrl[3])
        Gkd = float(valctrl[4])

#        Gpid.coeficientes (Gkp, Gki, Gkd)

        print ("Coeficientes ótimos = " + str(Gkp) + " " + str(Gki) + " " + str(Gkd))

    except:
        Gkp , Gki , Gkd = 0.00158366 , 0 , 1

        print ("Coeficientes >>arbitrários<< = " + str(Gkp) + " " + str(Gki) + " " + str(Gkd))



try:
    arq2 = open('Reference.csv', 'r')
    GDados = [float(x) for x in arq2.read().split(',')]

except:
    print ("Erro ao abrir arquivo de dados.")
    sys.exit (-1)



nivel = []

for GTempo in range(0,2000):

   AtualizarReator ()

   nivel.append (GReagenteTotal)


soma = 0
a    = 0
n    = len (nivel)

for k in range (1, n):
   a = nivel[k] - GDados[k]
   soma = soma + (a*a)

print (sqrt (soma / n))

if Gflagopt:
   sys.exit (0)


try:
   with open("ctrl.csv", "w", newline='') as arqout : # open('file.csv', newline='') as f:
       escritorcsv    = csv.writer(arqout)
       escritorcsv.writerow (Glistctrl)
except:
   print ("Erro ao abrir arquivo csv de controle para escrita")


plt.plot (nivel)

plt.title  ('Reference', fontsize=14)
plt.xlabel ('Time'     , fontsize=14)
plt.ylabel ('Solute concentration', fontsize=14)

ax = plt.gca()

#ax.set_xlim([xmin, xmax])
#ax.set_ylim([950, 1050])

for item in (\
             #[ax.title,ax.xaxis.label, ax.yaxis.label] + \
             ax.get_xticklabels() + ax.get_yticklabels()):

    item.set_fontsize(12)




plt.show ()
quit()


### plot results
fig,axs = plt.subplots(2,1,figsize=(8,5),gridspec_kw={'hspace':0.05})
ts = np.arange(0, npassos)
axs[0].plot(ts, entradas, label='entrada')
axs[0].plot(ts, saidas, label='saida')
axs[1].plot(ts, alvos, label='alvo')
axs[1].plot(ts, niveis, label='volume')
axs[1].set_xlabel('tempo')
axs[0].legend()
axs[1].legend()
axs[0].set_ylabel('Flow (MAF/year)')
axs[1].set_ylabel('Storage (MAF)')

plt.show()

